
// import haxe.ds.Array;
// import oimo.m.M;

import { BvhNode } from "./BvhNode";
import { M } from "../../../common/M";
import { BvhProxy } from "./BvhProxy";
import { BvhStrategy } from "./BvhStrategy";

/**
 * numberernal class.
 *
 * BVH Tree
 */
export class BvhTree {
	_root: BvhNode;
	_numLeaves: number;
	_strategy: BvhStrategy;

	_nodePool: BvhNode;

	leafList: BvhNode;
	leafListLast: BvhNode;

	tmp: Array<BvhNode>;

	constructor() {
		this._root = null;
		this._numLeaves = 0;
		this._strategy = new BvhStrategy();
		this._nodePool = null;
		this.leafList = null;
		this.leafListLast = null;
		this.tmp = new Array<BvhNode>(1024);
	}

	/**
	 * Inserts the proxy.
	 * This creates a leaf connected to the proxy and inserts it to the tree and `leafList`.
	 */

	public _insertProxy(proxy: BvhProxy): void {
		var leaf: BvhNode = this.pick();
		leaf._proxy = proxy;
		proxy._leaf = leaf;

		M.vec3_assign(leaf._aabbMin, proxy._aabbMin);
		M.vec3_assign(leaf._aabbMax, proxy._aabbMax);

		this._numLeaves++;
		if (this.leafList == null) {
			this.leafList = leaf;
			this.leafListLast = leaf;
		} else {
			this.leafListLast._nextLeaf = leaf;
			leaf._prevLeaf = leaf;
			this.leafListLast = leaf;
		}
		this.insertLeaf(leaf);
	}

	/**
	 * Deletes the proxy.
	 * This also deletes the leaf connected to the proxy from the tree and `leafList`.
	 */

	public _deleteProxy(proxy: BvhProxy): void {
		var leaf: BvhNode = proxy._leaf;

		this._numLeaves--;
		let prev = leaf._prevLeaf;
		let next = leaf._nextLeaf;
		if (prev != null) {
			prev._nextLeaf = next;
		}
		if (next != null) {
			next._prevLeaf = prev;
		}
		if (leaf == this.leafList) {
			this.leafList = next;
		}
		if (leaf == this.leafListLast) {
			this.leafListLast = prev;
		}
		leaf._prevLeaf = null;
		leaf._nextLeaf = null;

		this.deleteLeaf(leaf);

		proxy._leaf = null;
		this.pool(leaf);
	}

	/**
	 * Clears whole the tree.
	 * All leaves are disposed and deleted from `leafList`.
	 */

	public _clear(): void {
		if (this._root == null) return;
		this.deleteRecursive(this._root);
		this._root = null;
		this._numLeaves = 0;
	}


	public _optimize(count: number): void {
		if (this._root == null) return;
		for (let i = 0; i < count; ++i) {
			var leaf: BvhNode = this._root;
			while (leaf._height > 0) {
				var h1: number = leaf._children[0]._height;
				var h2: number = leaf._children[1]._height;
				// TODO: better strategy
				leaf = leaf._children[Math.random() > (h1 / (h1 + h2)) ? 1 : 0];
			}
			this.deleteLeaf(leaf);
			this.insertLeaf(leaf);
		}
	}


	public _buildTopDown(): void {
		if (this._root == null) return;
		this.decompose();

		// collect leaves
		var idx: number = 0;
		var leaf: BvhNode = this.leafList;
		while (leaf != null) {
			this.tmp[idx] = leaf;
			idx++;
			leaf = leaf._nextLeaf;
		}

		this._root = this.buildTopDownRecursive(this.tmp, 0, this._numLeaves);
	}

	public _getBalance(): number {
		return this.getBalanceRecursive(this._root);
	}

	// --- ---

	/**
	 * Makes the tree empty, but leaf nodes are not disposed and are reusable.
	 * The tree must be reconstructed using `leafList` after the call of this method.
	 */

	private decompose(): void {
		if (this._root == null) return;
		this.decomposeRecursive(this._root);
		this._root = null;
	}

	private deleteRecursive(root: BvhNode): void {
		if (root._height == 0) {
			let prev = root._prevLeaf;
			let next = root._nextLeaf;
			if (prev != null) {
				prev._nextLeaf = next;
			}
			if (next != null) {
				next._prevLeaf = prev;
			}
			if (root = this.leafList) {
				this.leafList = next;
			}
			if (root == this.leafListLast) {
				this.leafListLast = prev;
			}
			root._prevLeaf = null;
			root._nextLeaf = null;

			root._proxy._leaf = null;
			this.pool(root);
			return;
		}
		this.deleteRecursive(root._children[0]);
		this.deleteRecursive(root._children[1]);
		this.pool(root);
	}

	public decomposeRecursive(root: BvhNode): void {
		if (root._height == 0) {
			root._childIndex = 0;
			root._parent = null;
			return;
		}
		this.decomposeRecursive(root._children[0]);
		this.decomposeRecursive(root._children[1]);
		this.pool(root);
	}

	public buildTopDownRecursive(leaves: Array<BvhNode>, from: number, until: number): BvhNode {
		var num: number = until - from;
		M.assert(num > 0);
		if (num == 1) {
			var leaf: BvhNode = leaves[from];
			var proxy: BvhProxy = leaf._proxy;
			M.vec3_assign(leaf._aabbMin, proxy._aabbMin);
			M.vec3_assign(leaf._aabbMax, proxy._aabbMax);
			return leaf;
		}
		// sort and split
		var splitAt: number = this._strategy._splitLeaves(leaves, from, until);
		var child1: BvhNode = this.buildTopDownRecursive(leaves, from, splitAt);
		var child2: BvhNode = this.buildTopDownRecursive(leaves, splitAt, until);
		var parent: BvhNode = this.pick();
		parent._setChild(0, child1);
		parent._setChild(1, child2);
		parent._computeAabb();
		parent._computeHeight();
		return parent;
	}

	public getBalanceRecursive(root: BvhNode): number {
		if (root == null || root._height == 0) return 0;
		var balance: number = root._children[0]._height - root._children[1]._height;
		if (balance < 0) balance = -balance;
		return balance + this.getBalanceRecursive(root._children[0]) + this.getBalanceRecursive(root._children[1]);
	}


	private insertLeaf(leaf: BvhNode): void {
		this.assertBeLeaf(leaf);
		if (this._root == null) { // the tree is empty
			this._root = leaf;
			return;
		}
		// search for the position to insert
		var sibling: BvhNode = this._root;

		while (sibling._height > 0) {
			var nextStep: number = this._strategy._decideInsertion(sibling, leaf);

			if (nextStep == -1) {
				// insert to current position
				break;
			} else {
				sibling = sibling._children[nextStep];
			}
		}

		var parent: BvhNode = sibling._parent;

		// new common parent with the sibling
		var node: BvhNode = this.pick();

		if (parent == null) {
			// replace the root node
			this._root = node;
		} else {
			// connect to the old parent
			parent._setChild(sibling._childIndex, node);
		}
		node._setChild(sibling._childIndex, sibling);
		node._setChild(sibling._childIndex ^ 1, leaf);

		// fix data
		while (node != null) {
			if (this._strategy._balancingEnabled) {
				node = this.balance(node);
			}
			node._computeHeight();
			node._computeAabb();
			node = node._parent;
		}
	}


	private deleteLeaf(leaf: BvhNode): void {
		this.assertBeLeaf(leaf);
		if (this._root == leaf) { // the tree has only the leaf
			this._root = null;
			return;
		}
		var parent: BvhNode = leaf._parent;
		var sibling: BvhNode = parent._children[leaf._childIndex ^ 1];
		var grandParent: BvhNode = parent._parent;
		if (grandParent == null) {
			sibling._parent = null;
			sibling._childIndex = 0;
			this._root = sibling;
			this.pool(parent);
			return;
		}
		sibling._parent = grandParent;
		grandParent._setChild(parent._childIndex, sibling);
		this.pool(parent);

		// fix data
		var node: BvhNode = grandParent;
		while (node != null) {
			if (this._strategy._balancingEnabled) {
				node = this.balance(node);
			}
			node._computeHeight();
			node._computeAabb();
			node = node._parent;
		}
	}

	/**
	 * Balances and returns the node at the same position of `node`.
	 */

	private balance(node: BvhNode): BvhNode {
		var nh: number = node._height;
		if (nh < 2) {
			return node;
		}
		var p: BvhNode = node._parent;
		var l: BvhNode = node._children[0];
		var r: BvhNode = node._children[1];
		var lh: number = l._height;
		var rh: number = r._height;
		var balance: number = lh - rh;
		var nodeIndex: number = node._childIndex;

		//          [ N ]
		//         /     \
		//    [ L ]       [ R ]
		//     / \         / \
		// [L-L] [L-R] [R-L] [R-R]

		// is the tree balanced?
		if (balance > 1) {
			var ll: BvhNode = l._children[0];
			var lr: BvhNode = l._children[1];
			var llh: number = ll._height;
			var lrh: number = lr._height;

			// is L-L higher than L-R?
			if (llh > lrh) {
				// set N to L-R
				l._setChild(1, node);

				//          [ L ]
				//         /     \
				//    [L-L]       [ N ]
				//     / \         / \
				// [...] [...] [ L ] [ R ]

				// set L-R
				node._setChild(0, lr);

				//          [ L ]
				//         /     \
				//    [L-L]       [ N ]
				//     / \         / \
				// [...] [...] [L-R] [ R ]

				// fix bounds and heights
				l._computeAabb();
				l._computeHeight();
				node._computeAabb();
				node._computeHeight();
			} else {
				// set N to L-L
				l._setChild(0, node);

				//          [ L ]
				//         /     \
				//    [ N ]       [L-R]
				//     / \         / \
				// [ L ] [ R ] [...] [...]

				// set L-L
				node._setChild(0, ll);

				//          [ L ]
				//         /     \
				//    [ N ]       [L-R]
				//     / \         / \
				// [L-L] [ R ] [...] [...]

				// fix bounds and heights
				l._computeAabb();
				l._computeHeight();
				node._computeAabb();
				node._computeHeight();
			}
			// set new parent of L
			if (p != null) {
				p._setChild(nodeIndex, l);
			} else {
				this._root = l;
				l._parent = null;
			}
			return l;
		}
		if (balance < -1) {
			var rl: BvhNode = r._children[0];
			var rr: BvhNode = r._children[1];
			var rlh: number = rl._height;
			var rrh: number = rr._height;

			// is R-L higher than R-R?
			if (rlh > rrh) {
				// set N to R-R
				r._setChild(1, node);

				//          [ R ]
				//         /     \
				//    [R-L]       [ N ]
				//     / \         / \
				// [...] [...] [ L ] [ R ]

				// set R-R
				node._setChild(1, rr);

				//          [ R ]
				//         /     \
				//    [R-L]       [ N ]
				//     / \         / \
				// [...] [...] [ L ] [R-R]

				// fix bounds and heights
				r._computeAabb();
				r._computeHeight();
				node._computeAabb();
				node._computeHeight();
			} else {
				// set N to R-L
				r._setChild(0, node);

				//          [ R ]
				//         /     \
				//    [ N ]       [R-R]
				//     / \         / \
				// [ L ] [ R ] [...] [...]

				// set R-L
				node._setChild(1, rl);

				//          [ R ]
				//         /     \
				//    [ N ]       [R-R]
				//     / \         / \
				// [ L ] [R-L] [...] [...]

				// fix bounds and heights
				r._computeAabb();
				r._computeHeight();
				node._computeAabb();
				node._computeHeight();
			}
			// set new parent of R
			if (p != null) {
				p._setChild(nodeIndex, r);
			} else {
				this._root = r;
				r._parent = null;
			}
			return r;
		}
		return node;
	}


	private assertBeLeaf(leaf: BvhNode): void {
		M.assert(leaf._proxy != null && leaf._proxy._leaf == leaf && leaf._children[0] == null && leaf._children[1] == null && leaf._height == 0);
	}


	private pool(node: BvhNode): void {
		M.assert(node._proxy == null || node._proxy._leaf == null);
		node._removeReferences();
		node._nextLeaf = this._nodePool;
		this._nodePool = node;
	}


	private pick(): BvhNode {
		if (this._nodePool == null) {
			return new BvhNode();
		} else {
			var result = this._nodePool;
			this._nodePool = this._nodePool._next;
			result._next = null;
			return result;
		}
	}

}
